#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

/*
    需求 1 , 换算turtle1 与 turtle2 的换算关系
*/

int main(int argc, char ** argv)
{
    // 2，编码， 初始化， NodeHandle；
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "mlt_pub");
    ros::NodeHandle nh;
    //3，创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    //A创建发布对象：
    ros::Publisher pub=nh.advertise<geometry_msgs::Twist>("/trutle2/cmd_vel", 100);
    ros::Rate rate(10);
    while(ros::ok())
    {
        //核心
        try{
            //1，计算son1与son2的相对关系，
            /*
            参数1：目标坐标系
            参数2：原坐标系
            参数3： ros：：Time（0）； 最近时间
            返回值：geomotry——msgs：：transformStamped 原相对于目标的关系
            */
            geometry_msgs::TransformStamped turtle1Toturtle2 =buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
            // ROS_INFO("son1 相对于 son2 的信息： 父级 %s, 子级 %s, 偏移量（%.2f, %.2f, %.2f",
            //         son1Toson2.header.frame_id.c_str(), //turtle2
            //         son1Toson2.child_frame_id.c_str(),  //turtle1
            //         son1Toson2.transform.translation.x,
            //         son1Toson2.transform.translation.y,
            //         son1Toson2.transform.translation.z);
            
            //B 根据计算组织发布的消息
            geometry_msgs::Twist twist;
            twist.linear.x=0.5*sqrt(pow(turtle1Toturtle2.transform.translation.x, 2)+pow(turtle1Toturtle2.transform.translation.y, 2));
            twist.angular.z=4*atan2(turtle1Toturtle2.transform.translation.x,turtle1Toturtle2.transform.translation.y);
            //C 发布
            pub.publish(twist);
         }
         catch(const std::exception& e)
       {
           //std::cerr<<e.what()<<'/n';
           ROS_INFO("异常消息： %s", e.what());
       }
        rate.sleep();
        ros::spinOnce();

    }
    //5，spinOnce（）；


}